/*********************************************************************
* File: WaypointNavigation.cpp
*
* Implements the waypoint navigation API. 
*
* \author Instituto Superior Tecnico de Lisboa, Portugal
* \author Technical University of Lisbon, Portugal
* 
* \author Agentes Autonomos e Sistemas Multiagente
* \author Autonomous Agents and Multiagent Systems
* 
* \version	1.0
* \date		2006/2007
*********************************************************************/
#include "WaypointNavigation.h"
#include "../../util/mathLibrary.h"
#include "../../util/util.h"
#include "../../util/debug.h"
using namespace util;
#include "../../game/waypoint.h"
#include "../Agent.h"
using namespace agents;

#include <string>
using namespace std;

#pragma warning( disable : 4482 )  // Disable warning messages

/**
* \brief Class constructor.
*
* \param[in] owner The agent.
**/
WaypointNavigation::WaypointNavigation(Agent &owner)
:Navigation(owner)
{
	_waypointPathRootNode = NULL;
	_waypointPathCurrentNode = NULL;  
	_destinationWaypointIndex = -1;

	_isAutomaticWaypointNavigation = true;
	_moving = false;

	_costFunction = game::search::costFunctionPathDistance;
	_heuristicFunction = game::search::heuristicFunctionPathDistance;
	waypointHitHandler = NULL;
}

/**
* \brief Class destructor.
**/
WaypointNavigation::~WaypointNavigation()
{
	deletePath();
}
/**
* \brief Stops the agent.
*
* \see agents::interfaces::IWaypointNavigation::stop 
**/
void WaypointNavigation::stop()
{
	Navigation::stop();
	_moving = false;
	_currentWaypointIndex = -1;
}
/**
* \brief Holds the agent for a cycle.
*
* \see agents::interfaces::IWaypointNavigation::hold
**/
void WaypointNavigation::hold()
{
	Navigation::stop();
	_currentWaypointTimestamp = gpGlobals->time;
}

//BUG 01: Move to invisible waypoint in the reactive agent: Start
/**
* \brief Move to a specific waypoint as long as it is visible.
*
* \see agents::interfaces::IReactiveWaypointNavigation::moveToWaypoint 
* 
* \param waypointIndex The destination waypoint.
* \return True f path finding was successful, false otherwise.
**/
bool WaypointNavigation::moveToVisibleWaypoint(int waypointIndex)
{
	if(!game::waypoints::isValidIndex(waypointIndex))
		return false;
	if(!_owner.getVision().isPointVisible(getWaypointPosition(waypointIndex)))
		return false;

	return moveToWaypoint(waypointIndex);
}
//BUG 01: End
/**
* \brief Move to a specific waypoint.
*
* \see agents::interfaces::IWaypointNavigation::moveToWaypoint 
* 
* \param waypointIndex The destination waypoint.
* \return True f path finding was successful, false otherwise.
**/
bool WaypointNavigation::moveToWaypoint(int waypointIndex)
{
	if(findPath(_currentWaypointIndex, waypointIndex))
	{
		_destinationWaypointIndex = waypointIndex;
		_moving = true;
		return true;
	}
	else
		return false;
}
/**
* \brief Whether the destination waypoint has been reached.
*
* \see agents::interfaces::IWaypointNavigation::reachedDestinationWaypoint 
**/
bool WaypointNavigation::reachedDestinationWaypoint()
{
	return !_moving;
}


/**
* Gets visible waypoints.
*
* \see agents::interfaces::IReactiveWaypointNavigation::getVisibleWaypoints
**/
int WaypointNavigation::getVisibleWaypoints( int* waypoints, int max )
{
	int vIndex = 0;

	Waypoint* waypoint;
	for(int i=0; i<g_numWaypoints; i++)
	{
		waypoint = g_waypoints[i];
		if(_owner.getVision().isPointVisible(waypoint->origin))
			waypoints[vIndex++] = i;

		if(vIndex >= max)
			break;
	}

	return vIndex;
}
/**
* Gets visible goal waypoints.
*
* \see agents::interfaces::IReactiveWaypointNavigation::getVisibleGoalWaypoints
**/
int WaypointNavigation::getVisibleGoalWaypoints( int* waypoints, int max )
{
	return getVisibleWaypoints(waypoints, max, WaypointFlags::goal);
}
/**
* Gets visible terrorist important waypoints.
*
* \see agents::interfaces::IReactiveWaypointNavigation::getVisibleTerroristImportantWaypoints
**/
int WaypointNavigation::getVisibleTerroristImportantWaypoints( int* waypoints, int max )
{
	return getVisibleWaypoints(waypoints, max, WaypointFlags::terrorist);
}
/**
* Gets visible counter-terrorist important waypoints.
*
* \see agents::interfaces::IReactiveWaypointNavigation::getVisibleCounterTerroristImportantWaypoints
**/
int WaypointNavigation::getVisibleCounterTerroristImportantWaypoints( int* waypoints, int max )
{
	return getVisibleWaypoints(waypoints, max, WaypointFlags::counterTerrorist);
}
/**
* Gets visible camp waypoints.
*
* \see agents::interfaces::IReactiveWaypointNavigation::getVisibleCampWaypoints
**/
int WaypointNavigation::getVisibleCampWaypoints( int* waypoints, int max )
{
	return getVisibleWaypoints(waypoints, max, WaypointFlags::camp);
}
/**
* Gets visible waypoints with specified flags.
**/
int WaypointNavigation::getVisibleWaypoints( int* waypoints, int max, WaypointFlags::Enumeration flags )
{
	int vIndex = 0;

	Waypoint* waypoint;
	for(int i=0; i<g_numWaypoints; i++)
	{
		waypoint = g_waypoints[i];
		if(_owner.getVision().isPointVisible(waypoint->origin) && ((waypoint->flags & flags) != 0))
			waypoints[vIndex++] = i;

		if(vIndex >= max)
			break;
	}

	return vIndex;
}
/**
* Gets the waypoint's position.
*
* \see agents::interfaces::IReactiveWaypointNavigation::getWaypointPosition
**/
Vector WaypointNavigation::getWaypointPosition(int waypoint)
{
	return g_waypoints[waypoint]->origin;
}
/**
* Whether a waypoint is a goal waypoint.
*
* \see agents::interfaces::IReactiveWaypointNavigation::isGoalWaypoint
**/
bool WaypointNavigation::isGoalWaypoint(int waypoint)
{
	return (g_waypoints[waypoint]->flags & WaypointFlags::goal) != 0;
}
/**
* Whether a waypoint is a terrorist important waypoint.
*
* \see agents::interfaces::IReactiveWaypointNavigation::isTerroristImportantWaypoint
**/
bool WaypointNavigation::isTerroristImportantWaypoint(int waypoint)
{
	return (g_waypoints[waypoint]->flags & WaypointFlags::terrorist) != 0;
}
/**
* Whether a waypoint is a counter-terrorist important waypoint.
*
* \see agents::interfaces::IReactiveWaypointNavigation::isCounterTerroristImportantWaypoint
**/
bool WaypointNavigation::isCounterTerroristImportantWaypoint(int waypoint)
{
	return (g_waypoints[waypoint]->flags & WaypointFlags::counterTerrorist) != 0;
}
/**
* Whether a waypoint is a camp waypoint.
*
* \see agents::interfaces::IReactiveWaypointNavigation::isCampWaypoint
**/
bool WaypointNavigation::isCampWaypoint(int waypoint)
{
	return (g_waypoints[waypoint]->flags & WaypointFlags::camp) != 0;
}
/**
* \brief Gets the waypoints array.
*
* \see agents::interfaces::IWaypointNavigation::getWaypoints
**/
Waypoint** WaypointNavigation::getWaypoints()
{
	return g_waypoints;
}
/**
* \brief Gets the nuber of waypoints.
*
* \see agents::interfaces::IWaypointNavigation::getNumberOfWaypoints
**/
int WaypointNavigation::getNumberOfWaypoints()
{
	return g_numWaypoints;
}
/**
* \brief Gets the goal waypoint indices array.
*
* \see agents::interfaces::IWaypointNavigation::getGoalWaypointIndices
**/
int* WaypointNavigation::getGoalWaypointIndices()
{
	return g_goalWaypoints;
}
/**
* Gets goal waypoints count.
*
* \see agents::interfaces::IWaypointNavigation::getNumberOfGoalWaypoints
**/
int WaypointNavigation::getNumberOfGoalWaypoints()
{
	return g_numGoalWaypoints;
}
/**
* Gets the terrorist-important waypoint indices array.
*
* \see agents::interfaces::IWaypointNavigation::getTerroristImportantWaypointIndices
**/
int* WaypointNavigation::getTerroristImportantWaypointIndices()
{
	return g_terroristWaypoints;
}
/**
* Gets terrorist-important waypoints count.
*
* \see agents::interfaces::IWaypointNavigation::getNumberOfTerroristImportantWaypoints
**/
int WaypointNavigation::getNumberOfTerroristImportantWaypoints()
{
	return g_numTerroristWaypoints;
}
/**
* Gets the counter-terrorist-important waypoint indices array.
*
* \see agents::interfaces::IWaypointNavigation::getCounterTerroristImportantWaypointIndices
**/
int* WaypointNavigation::getCounterTerroristImportantWaypointIndices()
{
	return g_counterTerroristWaypoints;
}
/**
* Gets counter-terrorist-important waypoints count.
*
* \see agents::interfaces::IWaypointNavigation::getNumberOfCounterTerroristImportantWaypoints
**/
int WaypointNavigation::getNumberOfCounterTerroristImportantWaypoints()
{
	return g_numCounterTerroristWaypoints;
}
/**
* Gets the camp waypoint indices array.
*
* \see agents::interfaces::IWaypointNavigation::getCampWaypointIndices
**/
int* WaypointNavigation::getCampWaypointIndices()
{
	return g_campWaypoints;
}
/**
* Gets camp waypoints count.
*
* \see agents::interfaces::IWaypointNavigation::getNumberOfCampWaypoints
**/
int WaypointNavigation::getNumberOfCampWaypoints()
{
	return g_numCampWaypoints;
}
/** 
* \brief Resets variables for a new round.
*
* \see Navigation::newRound 
**/
void WaypointNavigation::newRound(){
	Navigation::newRound();

	stop();
	_currentWaypointOrigin = g_vecZero;
	_currentWaypointIndex = -1;
	_currentWaypointConnectionFlags = 0;
	_currentWaypointConnectionVelocity = g_vecZero;
	_previousWaypointIndices[0] = -1;
	_previousWaypointIndices[1] = -1;
	_previousWaypointIndices[2] = -1;
	_previousWaypointIndices[3] = -1;
	_previousWaypointIndices[4] = -1;
	_currentWaypointTimestamp = gpGlobals->time;
	_destinationWaypointIndex = -1;
}
/** 
* \brief Should be invoked at the start of each decision cycle.
*
* \see Navigation::startCycle 
**/
void WaypointNavigation::startCycle()
{
	Navigation::startCycle();

	validateCurrentWaypoint();

	if(_moving && _isAutomaticWaypointNavigation)
		if(update())
			stop();
}

/** 
* \brief Should be invoked at the end of each decision cycle.
*
* \see Navigation::endCycle 
**/
void WaypointNavigation::endCycle()
{
	if(game::waypoints::isValidIndex(_currentWaypointIndex))
		if ((g_waypoints[_currentWaypointIndex]->flags & WaypointFlags::crouch) && !(_owner.pev->button & IN_JUMP)) // Duck if required
			duck();

	Navigation::endCycle();
}
/** 
* Upon invoking the 'moveToWaypoint' function a path is created. This function 
* should be invoked at each cycle to lead the agent to the path's destination.
**/
bool WaypointNavigation::update()
{
	if (!game::waypoints::isValidIndex(_currentWaypointIndex))
		return false;

	bool bIsDucking = (_owner.pev->flags & FL_DUCKING) != 0;

	if(bIsDucking)
		moveToDestination(_currentWaypointOrigin);
	else
		moveToDestination(_currentWaypointOrigin + _owner.pev->view_ofs);

	float distanceToWaypoint = (_owner.pev->origin - _currentWaypointOrigin).Length();

	// Handle any travel flag this waypoint might have
	if (_currentWaypointConnectionFlags & WaypointConnectionFlags::jump)
	{
		// If Agent's on ground or on ladder we're free to jump.
		// Yes, I'm cheating here by setting the correct velocity
		// for the jump. Pressing the jump button gives the illusion
		// of the Agent actual jumping
		if (_owner.pev->flags & FL_ONGROUND || _owner.isOnLadder())
		{
			_owner.pev->velocity = _currentWaypointConnectionVelocity; //Cheat...
			jump();
			//_currentWaypointConnectionFlags &= ~WaypointConnectionFlags::jump;
		}
	}

	float waypointCutoffDistance = 50;
	_cutoffDistance = 15;
	// Initialize the radius for a special waypoint type, where the wpt
	// is considered to be reached
	if (bIsDucking || (g_waypoints[_currentWaypointIndex]->flags & WaypointFlags::goal))
		waypointCutoffDistance = 25;
	else if (g_waypoints[_currentWaypointIndex]->flags & WaypointFlags::ladder)
	{
		waypointCutoffDistance = 15; //Needs precise placement
		_cutoffDistance = 5;
	}

	int index = _currentWaypointIndex;

	// Check if Waypoint has a special travel flag, so they
	// need to be reached more precisely
	for (int i = 0; i < MAX_PATH_INDEX; i++)
	{
		if (g_waypoints[index]->connectflag[i] != 0)
		{
			waypointCutoffDistance = 0;
			break;
		}
	}

	// Needs precise placement - check if we get past the point
	if (waypointCutoffDistance < 16 && distanceToWaypoint < 30)
	{
		Vector positionInNextFrame = _owner.pev->origin;
		positionInNextFrame = positionInNextFrame + _owner.pev->velocity * g_frameDuration;
		float fDistanceInNextFrame = (positionInNextFrame - _currentWaypointOrigin).Length();
		if (fDistanceInNextFrame > distanceToWaypoint)				//If in the next frame, the agent is further
			waypointCutoffDistance = distanceToWaypoint + 1;	//away from the waypoint, then consider it
																	//to have already been reached
	}

	if (distanceToWaypoint < waypointCutoffDistance)
	{
		if(waypointHitHandler != NULL)
			waypointHitHandler->waypointHit(_currentWaypointIndex);

		_currentWaypointConnectionFlags = 0;

		// Did agent reach a destination waypoint?
		if (_destinationWaypointIndex == _currentWaypointIndex)
			return true;

		//Did agent reach end of path?
		else if (_waypointPathCurrentNode == NULL) 
			return false;

		// Do the actual movement checking
		advanceToNextWaypointInPath();
	}

	return false;
}
/**
* \brief Sets the closest current waypoint index.
* 
* \param index The waypoint index.
**/
void WaypointNavigation::setCurrentWaypointIndex(int index)
{
	_currentWaypointIndex = index;
	_currentWaypointOrigin = g_waypoints[_currentWaypointIndex]->origin;

	// If wayzone radius > 0 vary origin a bit depending on body angles
	if (g_waypoints[_currentWaypointIndex]->radius > 0)
	{
		util::math::makeVectors(
			Vector(_owner.pev->angles.x, 
			util::math::angleNormalize(_owner.pev->angles.y + RANDOM_FLOAT(-90, 90)), 0));
		_currentWaypointOrigin = 
			_currentWaypointOrigin 
			+ gpGlobals->v_forward * RANDOM_FLOAT(0, g_waypoints[_currentWaypointIndex]->radius);
	}

	_currentWaypointTimestamp = gpGlobals->time;

	if (game::waypoints::isValidIndex(_currentWaypointIndex))
		_currentWaypointFlags = g_waypoints[_currentWaypointIndex]->flags;
	else
		_currentWaypointFlags = 0;
}
/**
* \brief Gets the closest reachable waypoint to the agent.
**/
int WaypointNavigation::getClosestReachableWaypoint()
{
	int waypointIndex = -1;
	float distanceToWaypoint;
	float minimumDistanceToWaypoint = FLT_MAX;

	for (int i = 0; i < g_numWaypoints; i++)
	{
		// ignore current waypoint and previous recent waypoints...
		if (i == _currentWaypointIndex 
			|| i == _previousWaypointIndices[0] 
		|| i == _previousWaypointIndices[1] 
		|| i == _previousWaypointIndices[2] 
		|| i == _previousWaypointIndices[3] 
		|| i == _previousWaypointIndices[4])
			continue;

		if(game::waypoints::isReachable(_owner.pev->origin, g_waypoints[i]->origin, _owner.isInWater()))
		{
			distanceToWaypoint = util::math::lengthSquared(g_waypoints[i]->origin - _owner.pev->origin);

			if(distanceToWaypoint < minimumDistanceToWaypoint)
			{
				waypointIndex = i;
				minimumDistanceToWaypoint = distanceToWaypoint;
			}
		}
	}

	_previousWaypointIndices[4] = _previousWaypointIndices[3];
	_previousWaypointIndices[3] = _previousWaypointIndices[2];
	_previousWaypointIndices[2] = _previousWaypointIndices[1];
	_previousWaypointIndices[1] = _previousWaypointIndices[0];
	_previousWaypointIndices[0] = _currentWaypointIndex;

	_currentWaypointTimestamp = gpGlobals->time;
	return waypointIndex;
}

/**
* \brief Validates the current waypoint. 
*
* Checks if the last waypoint the agent was heading for is
* still valid. If it is not, sets the current waypoint to
* the closest reachable one
**/
void WaypointNavigation::validateCurrentWaypoint()
{
	// If agent hasn't got a waypoint a new one should be defined.
	// Current waypoint information is refreshed 5 secs after it has been defined.
	// Current waypoint should only be updated if no path is being followed
	if (!game::waypoints::isValidIndex(_currentWaypointIndex) ||
		((_currentWaypointTimestamp + 5.0 < gpGlobals->time) && (_waypointPathRootNode == NULL)))
	{
		int index  = getClosestReachableWaypoint();
		if(!game::waypoints::isValidIndex(index))
			index = RANDOM_LONG(0, g_numWaypoints - 1);

		setCurrentWaypointIndex(index);
	}
}

/**
* \brief Finds a path between two waypoints.
*
* Tries first the A* informed search algorithm and, if it fails, 
* the Floyd-Warshall shortest-path.
*
* \param startIndex The source waypoint index.
* \param endIndex The destination waypoint index.
* \return True if path finding was successful, false otherwise.
**/
bool WaypointNavigation::findPath(int startIndex, int endIndex)
{
	deletePath(); //Immediately cease to pursue the current path

	//Check path finding parameters
	if(!game::waypoints::isValidIndex(startIndex))
		return false;
	if(!game::waypoints::isValidIndex(endIndex))
		return false;
	if(startIndex == endIndex)
		return false;

	//First, try A* informed search algorithm
	game::search::PathNode* rootNode;
	if((_costFunction != NULL) && (_heuristicFunction != NULL))
		rootNode = game::search::aStar(startIndex, endIndex, _costFunction, _heuristicFunction);

	//If A* didn't find a path, try Floyd-Warshall shortest path algorithm
	if(rootNode == NULL)
		rootNode = game::search::floydWarshallShortestPath(startIndex, endIndex);

	if(rootNode == NULL)
		return false;
	//}
	else
	{
		_waypointPathRootNode = rootNode;
		_waypointPathCurrentNode = _waypointPathRootNode;
	}

	return true;
}

/**
* \brief Deletes the current path.
**/
void WaypointNavigation::deletePath()
{
	game::search::PathNode *currentNode, *deleteNode;

	currentNode = _waypointPathRootNode;
	while (currentNode != NULL)
	{
		deleteNode = currentNode->NextNode;
		free(currentNode);
		currentNode = deleteNode;
	}

	_waypointPathRootNode = NULL;
	_waypointPathCurrentNode = NULL;
}

/**
* \brief Whether the current goal waypoint is valid.
**/
bool WaypointNavigation::goalIsValid(void)
{
	if (!game::waypoints::isValidIndex(_destinationWaypointIndex)) // Not decided about a goal
		return false;
	else if (_destinationWaypointIndex == _currentWaypointIndex) // No Nodes needed
		return true;
	else if (_waypointPathCurrentNode == NULL) // No Path calculated
		return false;

	// Got Path - check if still valid
	game::search::PathNode *currentNode = _waypointPathCurrentNode;
	while (currentNode->NextNode != NULL)
		currentNode = currentNode->NextNode;

	return (currentNode->iIndex == _destinationWaypointIndex);
}
/**
* \brief Advances to the next node in the path the agent is following.
**/
bool WaypointNavigation::advanceToNextWaypointInPath()
{
	// No waypoints from pathfinding?
	if (_waypointPathCurrentNode == NULL)
		return false;

	// Advance to next node in path
	_waypointPathCurrentNode = _waypointPathCurrentNode->NextNode;

	// Reset travel flags(jumping etc)
	_currentWaypointConnectionFlags = 0;

	// We're not at the end of the path?
	if (_waypointPathCurrentNode != NULL)
	{
		int nextWaypointIndex = _waypointPathCurrentNode->iIndex;

		// Find out about connection flags
		if (game::waypoints::isValidIndex(_currentWaypointIndex))
		{
			Waypoint *waypoint = g_waypoints[_currentWaypointIndex];
			int i = 0;
			while (i < MAX_PATH_INDEX)
			{
				if (waypoint->index[i] == nextWaypointIndex)
				{
					_currentWaypointConnectionFlags = waypoint->connectflag[i];
					_currentWaypointConnectionVelocity = waypoint->vecConnectVel[i];
					break;
				}
				i++;
			}
		}
		setCurrentWaypointIndex(nextWaypointIndex);
	}

	// Is it a ladder? Then we need to adjust the waypoint origin
	// to make sure agent doesn't face wrong direction
	if (g_waypoints[_currentWaypointIndex]->flags & WaypointFlags::ladder)
	{
		edict_t *pLadder = NULL;

		// Find the ladder entity
		while (!FNullEnt(pLadder = FIND_ENTITY_IN_SPHERE(pLadder, _currentWaypointOrigin, 100)))
		{
			if (strcmp("func_ladder", STRING(pLadder->v.classname)) == 0)
				break;
		}

		// This Routine tries to find the direction the ladder is facing, so
		// I can adjust the ladder wpt origin to be some units into the direction the agent wants
		// to move because otherwise the Agent would turn all the time while climbing

		if (!FNullEnt(pLadder))
		{
			TraceResult tr;
			util::traceModel(_currentWaypointOrigin, util::vecBModelOrigin(pLadder), point_hull, pLadder, &tr);

			if (_currentWaypointOrigin.z < _owner.pev->origin.z)
			{
				// WPT is below Agent, so we need to climb down and need
				// to move wpt origin in the forward direction of the ladder
				setLadderMovementDirection(Navigation::LadderMovementDirections::down);
				_currentWaypointOrigin = _currentWaypointOrigin + tr.vecPlaneNormal;
			}
			else
			{
				// WPT is above Agent, so we need to climb up and need
				// to move wpt origin in the backward direction of the ladder
				setLadderMovementDirection(Navigation::LadderMovementDirections::up);
				_currentWaypointOrigin = _currentWaypointOrigin - tr.vecPlaneNormal;
			}
		}
	}

	return true;
}

